﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using MathNet.Numerics.LinearAlgebra.Generic;
using MathNet.Numerics.LinearAlgebra.Double;
using GestureSaver;

namespace CubeRotation
{
    public class MathTools
    {
        static Point XAxis = new Point(1, 0, 0);
        static Point YAxis = new Point(0, 1, 0);
        static Point ZAxis = new Point(0, 0, 1);

        public static Point RotateX(Point p, double angleX)
        {
            Matrix<double> rMatrixX = new DenseMatrix(3, 3);
            Vector<double> temp = p.V();

            rMatrixX[0, 0] = 1;
            rMatrixX[0, 1] = 0;
            rMatrixX[0, 2] = 0;
            rMatrixX[1, 0] = 0;
            rMatrixX[1, 1] = Math.Cos(angleX);
            rMatrixX[1, 2] = -Math.Sin(angleX);
            rMatrixX[2, 0] = 0;
            rMatrixX[2, 1] = Math.Sin(angleX);
            rMatrixX[2, 2] = Math.Cos(angleX);

            temp = rMatrixX * temp;

            return temp.ToPoint();
        }

        public static Point RotateY(Point p, double angleY)
        {
            Matrix<double> rMatrixY = new DenseMatrix(3, 3);
            Vector<double> temp = p.V();

            rMatrixY[0, 0] = Math.Cos(angleY);
            rMatrixY[0, 1] = 0;
            rMatrixY[0, 2] = Math.Sin(angleY);
            rMatrixY[1, 0] = 0;
            rMatrixY[1, 1] = 1;
            rMatrixY[1, 2] = 0;
            rMatrixY[2, 0] = -1 * Math.Sin(angleY);
            rMatrixY[2, 1] = 0;
            rMatrixY[2, 2] = Math.Cos(angleY);

            temp = rMatrixY * temp;

            return temp.ToPoint();
        }

        public static Point RotateZ(Point p, double angleZ)
        {
            Matrix<double> rMatrixZ = new DenseMatrix(3, 3);
            Vector<double> temp = p.V();

            rMatrixZ[0, 0] = Math.Cos(angleZ);
            rMatrixZ[0, 1] = -Math.Sin(angleZ);
            rMatrixZ[0, 2] = 0;
            rMatrixZ[1, 0] = Math.Sin(angleZ);
            rMatrixZ[1, 1] = Math.Cos(angleZ);
            rMatrixZ[1, 2] = 0;
            rMatrixZ[2, 0] = 0;
            rMatrixZ[2, 1] = 0;
            rMatrixZ[2, 2] = 1;

            temp = rMatrixZ * temp;

            return temp.ToPoint();
        }

        public static Point Translate(Point p, double x, double y, double z)
        {
            return new Point(p.X + x, p.Y + y, p.Z + z);
        }

        public static double GetAngle(Point p1, Point p2)
        {
            Vector<double> v1 = p1.V();
            Vector<double> v2 = p2.V();

            double sumTest = 0;

            double mag1, mag2, angle;

            for (int i = 0; i < 3; i++)
            {
                sumTest += v1[i] * v2[i];
            }

            mag1 = Magnitude(v1);
            mag2 = Magnitude(v2);

            angle = Math.Acos(sumTest / (mag2 * mag1));

            return angle;
        }

        public static double GetAngleX(Point p)
        {
            Point temp = DeepCloner.DeepClone<Point>(p);
            temp.X = 0;

            double angle = GetAngle(temp, YAxis);
            angle = temp.Y > 0 ? angle : -angle;

            return angle;
        }

        public static double GetAngleY(Point p)
        {
            Point temp = DeepCloner.DeepClone<Point>(p);
            temp.Y = 0;

            double angle = GetAngle(temp, ZAxis);
            angle = temp.Z > 0 ? angle : -angle;

            return angle;
        }

        public static double GetAngleZ(Point p)
        {
            Point temp = DeepCloner.DeepClone<Point>(p);
            temp.Z = 0;

            double angle = GetAngle(temp, YAxis);
            angle = temp.Y > 0 ? angle : -angle;

            return angle;
        }

        public static double Magnitude(Vector<double> vector)
        {
            double sum = 0;
            for (int i = 0; i < vector.Count; i++)
            {
                sum += Math.Pow(vector[i], 2);
            }
            return Math.Sqrt(sum);
        }

        public static double ToDegree(double radian)
        {
            return (radian / (2 * Math.PI)) * 360;
        }

        public static double ToRadian(double degree)
        {
            return (degree / 360) * (2 * Math.PI);
        }

        public static Vector<double> Normalize(Vector<double> vector)
        {
            double normal = 0;
            for (int i = 0; i < vector.Count; i++)
            {
                normal += Math.Pow(vector[i], 2);
            }
            normal = Math.Sqrt(normal);

            for (int i = 0; i < vector.Count; i++)
            {
                vector[i] /= normal;
            }

            return vector;
        }


    }
}
